#ifdef KITSCHY_DEBUG_MEMORY 
#include "debug_memorymanager.h"
#endif

#ifdef WIN32
#include "windows.h"
#else
#include <sys/time.h>
#include <time.h>
#endif
#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "stdarg.h"
#include "math.h"
#include "assert.h"

#include "conio.h"

#include "BList.h"

#include "symbol.h"
#include "sort.h"
#include "ontology.h"
#include "featureterm.h"
#include "FTKBase.h"
#include "Ufeatureterm.h"
#include "AUfeatureterm.h"
#include "FT_refinement.h"
#include "FT_generalization.h"
	


List<FeatureTerm> *depends_on(FeatureTerm *action,List<FeatureTerm> *set_by_condition,List<FeatureTerm> *set_by_action);
void show_plan_status(List<FeatureTerm> *head,List<FeatureTerm> *tail,FTKBase *domain_model);
void show_sorted_plan(List<FeatureTerm> *head,List<FeatureTerm> *set_by_condition,List<FeatureTerm> *set_by_action,FTKBase *domain_model);
bool restriction_violated(List<FeatureTerm> *head,List<FeatureTerm> *tail,Ontology *o,FTKBase *domain_model);



void plain_planner(FTKBase *domain_model, Ontology *o) 
{
	FeatureTerm *pattern=FeatureTerm::fromStringNOOS("(define (plotpoint))",domain_model,o);
	List<FeatureTerm> *actions=domain_model->RetrieveFT(pattern);
	List<FeatureTerm> tail_plan,move_to_head;
	List<FeatureTerm> head_plan;
	List<FeatureTerm> unsatisfied_conditions,unsafisfied_actions;
	FeatureTerm *action,*condition,*condition_to_satisfy=0,*action_to_satisfy;
	
	List<FeatureTerm> set_by_condition;
	List<FeatureTerm> set_by_action;

	List<FeatureTerm> head_plan_clone;
	List<FeatureTerm> tail_plan_clone;

	bool fail=false;

	delete pattern;

	printf("%i Basic actions to construct plotpoints\n",actions->Length());

	// Seed the algorithm:
	tail_plan.Add(FeatureTerm::fromStringNOOS("(define (plotpoint) (name \"endgame\") (preconditions (define (endgame))))",domain_model,o));

 	while(!tail_plan.EmptyP() && !fail) {
		show_plan_status(&head_plan,&tail_plan,domain_model);

		// STEP 0: clone the plan
		
		{
			FeatureTerm *plan_clone=FeatureTerm::SetFeatureTerm();
			FeatureTerm *tmp=FeatureTerm::SetFeatureTerm();
			List<FeatureTerm> *l;
			
			head_plan_clone.Delete();
			tail_plan_clone.Delete();

			head_plan.Rewind();
			while(head_plan.Iterate(action)) tmp->addSetValue(new FeatureTerm(action));
			plan_clone->addSetValue(tmp);
			tmp=FeatureTerm::SetFeatureTerm();
			tail_plan.Rewind();
			while(tail_plan.Iterate(action)) tmp->addSetValue(new FeatureTerm(action));
			plan_clone->addSetValue(tmp);

			tmp=plan_clone->clone(domain_model);

			l=tmp->set_values_quick()->operator [](0)->set_values_quick();
			l->Rewind();
			while(l->Iterate(action)) head_plan_clone.Add(new FeatureTerm(action));
			l=tmp->set_values_quick()->operator [](1)->set_values_quick();
			l->Rewind();
			while(l->Iterate(action)) tail_plan_clone.Add(new FeatureTerm(action));

			delete plan_clone;
			delete tmp;
		}
		

		// STEP 1, find unsatisfied conditions:		
		{
			List<FeatureTerm> pre_condition_list;
			FeatureTerm *pre_conditions;
			bool ready_for_head;

			tail_plan.Rewind();
			while(tail_plan.Iterate(action)) {
				ready_for_head=true;
				pre_conditions=action->featureValueQuick("preconditions");
				if (pre_conditions!=0) {
					if (pre_conditions->isSet()) {
						List<FeatureTerm> *l=pre_conditions->set_values_quick();
						l->Rewind();
						while(l->Iterate(condition)) pre_condition_list.Add(condition);
					} else {
						pre_condition_list.Add(pre_conditions);
					} // if
				} // if 

				pre_condition_list.Rewind();
				while(pre_condition_list.Iterate(condition)) {
					int k=set_by_condition.Position(condition);
					if (k==-1) {
						unsatisfied_conditions.Add(condition);
						unsafisfied_actions.Add(action);
						ready_for_head=false;
					} // if
				} // while

				pre_condition_list.ExtractAll();

				if (ready_for_head) move_to_head.Add(action);
			} // while
		}

		printf("Unsatisfied conditions:\n");
		unsatisfied_conditions.Rewind();
		while(unsatisfied_conditions.Iterate(condition)) {
			char *s=condition->toStringNOOS(domain_model);
			printf("%s\n",s);
			delete []s;
		} // while

		// STEP 2, select one unsatisfied condition:
		condition_to_satisfy=unsatisfied_conditions.GetRandom();
		printf("Condition selected: %i / %i\n",unsatisfied_conditions.PositionRef(condition_to_satisfy),unsatisfied_conditions.Length());
		action_to_satisfy=unsafisfied_actions[unsatisfied_conditions.PositionRef(condition_to_satisfy)];

		unsatisfied_conditions.ExtractAll();
		unsafisfied_actions.ExtractAll();

		if (condition_to_satisfy!=0) {
			List<FeatureTerm> actions_before,candidate_actions;
			FeatureTerm *candidate_action;

			{
				char *s=condition_to_satisfy->toStringNOOS(domain_model);
				printf("condition selected to satisfy:\n");
				printf("%s\n",s);
				delete []s;
			}

			// STEP 3: find actions that can occur before the current action:
			{
				int hc=0,tc=0,nc=0;
				List<FeatureTerm> *dependences;
				// all the head ones not linked directly or indirectly to the selected action:
				/*
				dependences = depends_on(action_to_satisfy,&set_by_condition,&set_by_action);
				head_plan.Rewind();
				while(head_plan.Iterate(action))
					if (!dependences->MemberP(action)) {
						actions_before.Add(action);
						hc++;
					} // if
				*/
				head_plan.Rewind();
				while(head_plan.Iterate(action)) {
					dependences = depends_on(action,&set_by_condition,&set_by_action);
					if (!dependences->MemberP(action_to_satisfy)) {
						actions_before.Add(action);
						hc++;
					} // if
					dependences->ExtractAll();
					delete dependences;
				}

				// all the new ones:
				actions->Rewind();
				while(actions->Iterate(action)) {
					actions_before.Add(action);
					nc++;
				} // while
				// all the tail ones not linked directly or indirectly to the selected action:
				tail_plan.Rewind();
				while(tail_plan.Iterate(action)) {
					dependences = depends_on(action,&set_by_condition,&set_by_action);
					if (!dependences->MemberP(action_to_satisfy)) {
						actions_before.Add(action);
						tc++;
					} // if
					dependences->ExtractAll();
					delete dependences;
				}
				printf("%i actions satisfy time constraints (%i - %i - %i) \n",hc,tc,nc);
			}

			// STEP 4: find actions to satisfy the condition:
			{
				actions_before.Rewind();
				while(actions_before.Iterate(action)) {
					bool candidate=false;
					List<FeatureTerm> post_condition_list;
					FeatureTerm *post_conditions,*tmp;

					post_conditions=action->featureValueQuick("postconditions");
					if (post_conditions!=0) {
						if (post_conditions->isSet()) {
							List<FeatureTerm> *l=post_conditions->set_values_quick();
							l->Rewind();
							while(l->Iterate(condition)) post_condition_list.Add(condition);
						} else {
							post_condition_list.Add(post_conditions);
						} // if
					} // if 

					while(!post_condition_list.EmptyP() && !candidate) {
						condition = post_condition_list.ExtractIni();

						tmp = simple_unification(condition,condition_to_satisfy,o,domain_model);
						if (tmp!=0) {
							if (!tmp->isSet()) candidate=true;
							delete tmp;
						} // if 

					} // while 

					if (candidate) {
						candidate_actions.Add(action);
					} // if 
				} // while 
			}
			printf("%i actions are candidates\n",candidate_actions.Length());


			actions_before.ExtractAll();
			// STEP 5: select one
			candidate_action = candidate_actions.ExtractRandom();
			candidate_actions.ExtractAll();

			if (candidate_action!=0) {
				{
					char *s=candidate_action->toStringNOOS(domain_model);
					printf("action selected to satisfy the condition:\n");
					printf("%s\n",s);
					delete []s;
				}

				// STEP 6: unify and update plan
				FeatureTerm *selected_condition=0;

				if (!head_plan.MemberP(candidate_action) &&
					!tail_plan.MemberP(candidate_action)) {
					candidate_action=candidate_action->clone();
					tail_plan.Add(candidate_action);
				} // if 

				{
					bool candidate=false;
					List<FeatureTerm> post_condition_list;
					FeatureTerm *post_conditions,*tmp;

					post_conditions=candidate_action->featureValueQuick("postconditions");
					if (post_conditions!=0) {
						if (post_conditions->isSet()) {
							List<FeatureTerm> *l=post_conditions->set_values_quick();
							l->Rewind();
							while(l->Iterate(condition)) post_condition_list.Add(condition);
						} else {
							post_condition_list.Add(post_conditions);
						} // if
					} // if 

					set_by_condition.Add(new FeatureTerm(condition_to_satisfy));
					set_by_action.Add(new FeatureTerm(candidate_action));

					while(!post_condition_list.EmptyP() && !candidate) {
						condition = post_condition_list.ExtractIni();

						tmp = simple_unification(condition,condition_to_satisfy,o,domain_model);
						if (tmp!=0) {
							delete tmp;
							candidate=true;
							selected_condition=condition;
						} // if 

					} // while 
				}

				List<UnificationResult> *l=0;
				UnificationResult *r=0;
				l = unification_with_bindings(selected_condition,condition_to_satisfy,o,domain_model);
				if (l!=0) r=l->ExtractIni();					

				if (r!=0) {
					UnificationBinding *b;

					r->m_bindings.Rewind();
					while(r->m_bindings.Iterate(b)) {
						head_plan.Rewind();
						while(head_plan.Iterate(action)) {
							if (b->m_f1!=0) action->substitute(b->m_f1,b->m_u);
							if (b->m_f2!=0) action->substitute(b->m_f2,b->m_u);
						} // while 
						tail_plan.Rewind();
						while(tail_plan.Iterate(action)) {
							if (b->m_f1!=0) action->substitute(b->m_f1,b->m_u);
							if (b->m_f2!=0) action->substitute(b->m_f2,b->m_u);
						} // while 
						set_by_condition.Rewind();
						while(set_by_condition.Iterate(condition)) {
							if (b->m_f1!=0) {
								if ((*(b->m_f1))==(*condition)) {
									set_by_condition.SetObj(set_by_condition.PositionRef(condition),new FeatureTerm(b->m_u));
									delete condition;
								} // if 
							} // if 
							if (b->m_f2!=0) {
								if ((*(b->m_f2))==(*condition)) {
									set_by_condition.SetObj(set_by_condition.PositionRef(condition),new FeatureTerm(b->m_u));
									delete condition;
								} // if 
							} // if 
						} // while 

					} // while 
					delete r;
				} else {
					printf("FAILURE: no unification context\n");
					fail=true;
				} // if 
				delete l;

			} else {
				printf("FAILURE: empty candidate action\n");
				fail=true;
			} // if

		} // if 

		// Check restrictions:
		if (restriction_violated(&head_plan,&tail_plan,o,domain_model)) {
			printf("Substituting plan by this one:\n");
			
			tail_plan.Delete();
			head_plan.Delete();

			while(!tail_plan_clone.EmptyP()) tail_plan.Add(tail_plan_clone.ExtractIni());
			while(!head_plan_clone.EmptyP()) head_plan.Add(head_plan_clone.ExtractIni());

		} else {
			// STEP 7: mode actions to the head
			if (!fail) {
				while(!move_to_head.EmptyP()) {
					action=move_to_head.ExtractIni();
					tail_plan.DeleteElement(action);
					head_plan.Add(action);
				} // while
			} // if 
		} // if 

		head_plan_clone.Delete();
		tail_plan_clone.Delete();

//		getch();
	} // while

	if (fail) printf("\nFailed to generate a complete story.\n\n");
	
	show_plan_status(&head_plan,&tail_plan,domain_model);

	if (!fail) show_sorted_plan(&head_plan,&set_by_condition,&set_by_action,domain_model);

	actions->ExtractAll();
	delete actions;
} /* plain_planner */ 


	
List<FeatureTerm> *depends_on(FeatureTerm *action,List<FeatureTerm> *set_by_condition,List<FeatureTerm> *set_by_action)
{
	List<FeatureTerm> to_process,pre_condition_list;
	List<FeatureTerm> *dependences = new List<FeatureTerm>;
	FeatureTerm *condition,*pre_conditions,*action2;

	to_process.Add(action);
	while(!to_process.EmptyP()) {
		action=to_process.ExtractIni();
		dependences->Add(action);

		pre_conditions=action->featureValueQuick("preconditions");
		if (pre_conditions!=0) {
			if (pre_conditions->isSet()) {
				List<FeatureTerm> *l=pre_conditions->set_values_quick();
				l->Rewind();
				while(l->Iterate(condition)) pre_condition_list.Add(condition);
			} else {
				pre_condition_list.Add(pre_conditions);
			} // if
		} // if 

		pre_condition_list.Rewind();
		while(pre_condition_list.Iterate(condition)) {
			int k=set_by_condition->Position(condition);
			if (k==-1) action2=0;
				  else action2=set_by_action->operator [](k);
			if (action2!=0) {
				if (!to_process.MemberP(action2) && !dependences->MemberP(action2)) to_process.Add(action2);

			} // if
		} // while
		pre_condition_list.ExtractAll();
	} // while 

	return dependences;
} /* depends_on */ 


void show_plan_status(List<FeatureTerm> *head,List<FeatureTerm> *tail,FTKBase *domain_model) 
{
	char *s;
	FeatureTerm *result,*action;

	printf("-------------------------------------------------------------\n");
	printf("Head plan (%i actions):\n",head->Length());
	result=FeatureTerm::SetFeatureTerm();
	head->Rewind();
	while(head->Iterate(action)) {
		result->addSetValue(new FeatureTerm(action));
	} // while
	s=result->toStringNOOS(domain_model);
	printf("%s\n",s);
	delete []s;
	delete result;

	printf("\nTail plan (%i actions):\n",tail->Length());
	result=FeatureTerm::SetFeatureTerm();
	tail->Rewind();
	while(tail->Iterate(action)) {
		result->addSetValue(new FeatureTerm(action));
	} // while

	s=result->toStringNOOS(domain_model);
	printf("%s\n\n",s);
	delete []s;

	delete result;

	fflush(0);

} /* show_plan_status */ 


bool restriction_violated(List<FeatureTerm> *head,List<FeatureTerm> *tail,Ontology *o,FTKBase *domain_model)
{
	FeatureTerm *action,*restriction;
	FeatureTerm *restrictions;
	List<FeatureTerm> restrictions_list;

	head->Rewind();
	while(head->Iterate(action)) {
		restrictions=action->featureValueQuick("restrictions");
		
		if (restrictions!=0) {
			if (restrictions->isFeatureTerm()) {
				restrictions_list.Add(restrictions);
			} else {
				FeatureTerm *r;
				List<FeatureTerm> *l=restrictions->set_values_quick();
				l->Rewind();
				while(l->Iterate(r)) restrictions_list.Add(r);
			} // if 
		} // if

	} // while 

	tail->Rewind();
	while(tail->Iterate(action)) {
		restrictions=action->featureValueQuick("restrictions");
		
		if (restrictions!=0) {
			if (restrictions->isFeatureTerm()) {
				restrictions_list.Add(restrictions);
			} else {
				FeatureTerm *r;
				List<FeatureTerm> *l=restrictions->set_values_quick();
				l->Rewind();
				while(l->Iterate(r)) restrictions_list.Add(r);
			} // if 
		} // if

	} // while 

	while(!restrictions_list.EmptyP()) {
		restriction=restrictions_list.ExtractIni();

		if (restriction->getSort()==o->get_sort("different")) {
			FeatureTerm *c1,*c2;

			c1=restriction->featureValueQuick("condition1");
			c2=restriction->featureValueQuick("condition2");

			if (*c1==*c2) {
				printf("/* ----------------------------- *\\\n");
				printf("Different restriction violated!:\n");
				{
					char *s;
					s=c1->toStringNOOS(domain_model);
					printf("c1: %s\n",s);
					delete []s;
					s=c2->toStringNOOS(domain_model);
					printf("c2: %s\n",s);
					delete []s;
				}
				printf("\\* ----------------------------- */\n");
				restrictions_list.ExtractAll();
				return true;
			} // if
		} // if 
	} // while 


	restrictions_list.ExtractAll();
	return false;
} /* restriction_violated */ 


void show_sorted_plan(List<FeatureTerm> *head,List<FeatureTerm> *set_by_condition,List<FeatureTerm> *set_by_action,FTKBase *domain_model)
{
	List<FeatureTerm> sorted_actions;
	List<FeatureTerm> unsorted_actions;
	FeatureTerm *action,*a2;
	List<FeatureTerm> *dependences;
	bool applicable=false,found;

	unsorted_actions.AddAll(head);

	while(!unsorted_actions.EmptyP()) {
		unsorted_actions.Rewind();

		found = false;
		while(!found && unsorted_actions.Iterate(action)) {
			dependences=depends_on(action,set_by_condition,set_by_action);

//			printf("Considering %i (%s) [%i dependences]\n",head->Position(action),action->featureValueQuick("name")->getSymbol()->get(),dependences->Length());

			applicable=true;
			while(!dependences->EmptyP() && applicable) {
				a2=dependences->ExtractIni();
				if (!sorted_actions.MemberP(a2) && !(*a2==*action)) applicable=false;
			} // whiel 

			dependences->ExtractAll();
			delete dependences;

			if (applicable) {
				found=true;

				printf("Next action is Action %i (%s)\n",head->Position(action),action->featureValueQuick("name")->getSymbol()->get());

				unsorted_actions.DeleteElement(action);
				sorted_actions.Add(action);
			} // if 
		} // while 

		if (!found) {
			sorted_actions.ExtractAll();
			unsorted_actions.ExtractAll();
			printf("Plan cannot be sorted!\n");
			return;
		} // if 
	} // while 


	{
		List<FeatureTerm> characters,places;
		FeatureTerm *c1,*c2,*c3;
		FeatureTerm *p1;


		printf("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
		printf("Sorted plan (%i actions):\n",head->Length());
		sorted_actions.Rewind();
		while(sorted_actions.Iterate(action)) {
			if (strcmp("endgame-rescue-character",
				action->featureValueQuick("name")->getSymbol()->get())==0) {
				c1=action->featureValueQuick("preconditions")->set_values_quick()->operator [](0);
				if (!characters.MemberP(c1)) characters.Add(c1);
				c2=action->featureValueQuick("preconditions")->set_values_quick()->operator [](2);
				if (!characters.MemberP(c2)) characters.Add(c2);
				p1=action->featureValueQuick("preconditions")->set_values_quick()->operator [](1);
				if (!places.MemberP(p1)) places.Add(p1);
				printf("The hero rescues C%i from C%i at P%i\n",characters.Position(c1),characters.Position(c2),places.Position(p1));
			} else if (strcmp("endgame-kill-character",
				action->featureValueQuick("name")->getSymbol()->get())==0) {
				c1=action->featureValueQuick("preconditions");
				if (!characters.MemberP(c1)) characters.Add(c1);
				printf("The hero kills C%i\n",characters.Position(c1));
			} else if (strcmp("rescue-character",
				action->featureValueQuick("name")->getSymbol()->get())==0) {
				c1=action->featureValueQuick("preconditions")->set_values_quick()->operator [](0);
				if (!characters.MemberP(c1)) characters.Add(c1);
				c2=action->featureValueQuick("preconditions")->set_values_quick()->operator [](2);
				if (!characters.MemberP(c2)) characters.Add(c2);
				p1=action->featureValueQuick("preconditions")->set_values_quick()->operator [](1);
				if (!places.MemberP(p1)) places.Add(p1);
				printf("The hero rescues C%i from C%i at P%i\n",characters.Position(c1),characters.Position(c2),places.Position(p1));
			} else if (strcmp("kidnap-character",
				action->featureValueQuick("name")->getSymbol()->get())==0) {
				c1=action->featureValueQuick("preconditions")->set_values_quick()->operator [](0);
				if (!characters.MemberP(c1)) characters.Add(c1);
				c2=action->featureValueQuick("preconditions")->set_values_quick()->operator [](2);
				if (!characters.MemberP(c2)) characters.Add(c2);
				p1=action->featureValueQuick("preconditions")->set_values_quick()->operator [](1);
				if (!places.MemberP(p1)) places.Add(p1);
				printf("C%i is kidnapped by C%i at P%i\n",characters.Position(c1),characters.Position(c2),places.Position(p1));
			} else if (strcmp("character-is-born",
				action->featureValueQuick("name")->getSymbol()->get())==0) {
				c1=action->featureValueQuick("preconditions")->set_values_quick()->operator [](0);
				if (!characters.MemberP(c1)) characters.Add(c1);
				c2=action->featureValueQuick("preconditions")->set_values_quick()->operator [](1);
				if (!characters.MemberP(c2)) characters.Add(c2);
				c3=action->featureValueQuick("postconditions");
				if (!characters.MemberP(c3)) characters.Add(c3);
				printf("C%i (%s) is born from C%i (%s) and C%i (%s) \n",characters.Position(c3),c3->getSort()->get(),
																		characters.Position(c1),c1->getSort()->get(),
																		characters.Position(c2),c2->getSort()->get());
			} else if (strcmp("character-exists",
				action->featureValueQuick("name")->getSymbol()->get())==0) {
				c1=action->featureValueQuick("postconditions");
				if (!characters.MemberP(c1)) characters.Add(c1);
				printf("C%i is a %s\n",characters.Position(c1),c1->getSort()->get());
			} else if (strcmp("location-is-build",
				action->featureValueQuick("name")->getSymbol()->get())==0) {
				c1=action->featureValueQuick("preconditions");
				if (!characters.MemberP(c1)) characters.Add(c1);
				p1=action->featureValueQuick("postconditions");
				if (!places.MemberP(p1)) places.Add(p1);
				printf("C%i (%s) builds P%i (%s) \n",characters.Position(c1),c1->getSort()->get(),	
													 places.Position(p1),p1->getSort()->get());


			} else if (strcmp("not-kidnapped-to-start",
				action->featureValueQuick("name")->getSymbol()->get())==0) {
			} else if (strcmp("endgame",
				action->featureValueQuick("name")->getSymbol()->get())==0) {
				printf("The story ends\n");
			} else {
				char *s=action->toStringNOOS(domain_model);
				printf("%s\n",s);
				delete []s;
			} // if
		} // while

		characters.ExtractAll();
		places.ExtractAll();
	}

	sorted_actions.ExtractAll();
	
}/* show_sorted_plan */ 
